#include "my_matrix3x3.h"
#include<iostream>

#include <gtest/gtest.h>

TEST(Quaternion2Euler, Quaternion2EulerTest1) {
  double roll,pitch,yaw;
  std::cout<<"输入的四元数：w,x,y,z:"; 
  double w,x,y,z;
  std::cin>>x>>y>>z>>w;
  my_tf::Quaternion RQ2(x,y,z,w);  
  my_tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw);  
  std::cout<<"输出的欧拉角为：roll="<<roll<<",pitch="<<pitch<<",yaw="<<yaw<<std::endl;
};